package nxt.WallRobo_03;
import lejos.nxt.*;
public class Usthread extends Thread
{	
	public double Drift = 0;
	public int wallside = 0;
	public boolean correction_in_progress = false;
	public Object wait_for_wallside = new Object();
	public int last_distance_distance = 0;
	public int last_distance_Tacho = 0;
	public int current_distance_distance = 0;
	public int current_distance_Tacho = 0;
	int degrees = 0;
	int counter = 0;
	public boolean correction_is_needed = false;
	public UltrasonicSensor us = new UltrasonicSensor(SensorPort.S1);
	public Usthread(){}
	public void run()
	{
		
//Wandabfrage
		
		LCD.drawString("Wand R oder L?", 0, 0);

		while (!Button.RIGHT.isPressed() && !Button.LEFT.isPressed())
		{
		}

		if (Button.RIGHT.isPressed()) 
		{
			wallside = 1;
		} 
		else if (Button.LEFT.isPressed()) 
		{
			wallside = -1;
		}

		try
		{
			synchronized(wait_for_wallside)
			{	
				wait_for_wallside.notifyAll();
			}
		}
		catch (Exception e){}

		correction_in_progress = false;
		us.continuous();
		while (true) 
		{			
			degrees = Motor.B.getTachoCount();
			// Messungen

			while ((degrees * wallside) > 80 && correction_in_progress == false)
			{
				us.ping();
				current_distance_distance = current_distance_distance + us.getDistance();
				counter = counter + 1;
				LCD.clear();
				degrees = Motor.B.getTachoCount();
					
			}
			
				// Durchschnittsberechnung, Setzung der Standarddistanz

			if (counter != 0 && correction_in_progress == false)
			{
				Sound.beep();
				current_distance_Tacho = Motor.A.getTachoCount();
				current_distance_distance = (int) current_distance_distance / counter;
				if (last_distance_distance == 0)
				{
					last_distance_distance = current_distance_distance;
					last_distance_Tacho = current_distance_Tacho;
					
				}
				else if (current_distance_distance != last_distance_distance)
				{


					Drift = (Math.toDegrees(Math.asin((current_distance_distance-last_distance_distance)/((current_distance_Tacho-last_distance_Tacho)*0.0698))))*wallside;
					
					correction_is_needed = true;
					if((current_distance_distance-last_distance_distance)/((current_distance_Tacho-last_distance_Tacho)*0.0698)>=1 ||(current_distance_distance-last_distance_distance)/((current_distance_Tacho-last_distance_Tacho)*0.0698) <=-1 )
					{
						Sound.beepSequence();
						Sound.buzz();
						System.exit(0);
					}
				}
				LCD.drawInt(current_distance_distance, 0, 1);
				LCD.drawInt(counter, 0, 0);
				LCD.drawInt((int)(Drift), 0, 2);
				counter = 0;
				last_distance_distance = current_distance_distance;
				last_distance_Tacho = current_distance_Tacho;
				current_distance_distance = 0;
				current_distance_Tacho = 0;
//				Motor.A.resetTachoCount();
				
			}

		}
		
	}	
}